#include "scheduler.h"



/**
 * Control flag register for main loop.
 */
volatile int sched_controlFlags = 0;

/*
 * Low priority tasks are executed after scheduled or interrupt driven tasks.
 * Tasks are removed from the queue after being executed.
 */
volatile TASK *lowPriorityTasks;
/**
 * High priority tasks, done before checking flags.
 * Tasks are removed from the queue after being executed.
 */
volatile TASK *highPriorityTasks;
/*
 * Synchronised task queue. Synchronised tasks are run following the end of a synchronised MAC frame.
 * Synchronised tasks are not removed after they are run.
 *
 * If the node is not synchronised then tasks are run periodically anyway.
 */
static volatile TASK *synchronisedTasks;
/*
 * Scheduled task queues.
 */
static volatile SCHEDULED_TASK *timerQueues[2];

/**
 * Lock for the synchronised task list to stop tasks being added to the list while it is being
 * processed.
 */
static boolean synchronisedQueueLock = false;

/*
 * The time of the last ISR that decremented tasks in CLOCK_PERIODS queue.
 */
volatile int lastInterruptTime;


volatile int currentTime;

volatile int nextSecInterrupt;

volatile int nextmsInterrupt;

boolean secDelayTimerActive = false;

boolean msDelayTimerActive = false;

/**
 * Initialise scheduler's variables
 */
void sched_init(void)
{
	timerQueues[0] = NULL;
	timerQueues[1] = NULL;
	highPriorityTasks = NULL;
	lowPriorityTasks = NULL;
	synchronisedTasks = NULL;
	secDelayTimerActive = false;
	msDelayTimerActive = false;
	currentTime = 0;
	nextSecInterrupt = 0;
	nextmsInterrupt = 0;
}

/**
 * Add tasks to be executed at certain times
 */
void sched_addTimedTask(SCHEDULED_TASK* task) {
	volatile SCHEDULED_TASK **queuePtrPtr;
	unsigned int nextInterruptTime, expiredTime, timeNow;
	boolean addTask;
	timeNow = currentTime; // get the count from the mainTimer
	addTask = true;
	task->nextTask = NULL;
	// Check time setup.
	if(task->remaining == 0) {
		if(task->period == 0)
			return; // Both remaining and period == 0, not a timer at all.
		task->remaining = task->period; 	// Set remaining to task period.
	}
	if((unsigned char)task->basePeriod > (unsigned char)SECONDS) {
		// Base Period is out of range
		return;
	}
	queuePtrPtr = &(timerQueues[task->basePeriod]); // Get the correct task queue
	// Add to queue.
	if(*queuePtrPtr == NULL) { // Empty queue
		*queuePtrPtr = task;
		(*queuePtrPtr)->nextTask = NULL;
		// Queue was empty set compare register and enable interrupt.
		switch (task->basePeriod) {
			case CLOCK_PERIODS:
				nextmsInterrupt = timeNow + task->remaining;
				lastInterruptTime = timeNow; // For calculating elapsed time.
				msDelayTimerActive = true;
				break;
			case SECONDS:
				nextSecInterrupt = timeNow + SECONDS_INCREMENT;
				secDelayTimerActive = true;
				break;
		}
	}
	else {
		switch (task->basePeriod) {
			case CLOCK_PERIODS:
				// Does the new task occur before the next interrupt time?
				nextInterruptTime = nextmsInterrupt;
				if(timeNow + task->remaining < nextInterruptTime) {
					nextInterruptTime = timeNow + task->remaining;
					expiredTime = timeNow - lastInterruptTime;
				}
				else {
					expiredTime = 0; // No adjustment to make.
				}
				// Will be set after, only if task is newly added to queue
				break;
			case SECONDS:
				break;
		}
		// Loop to end of queue.
		for(; (*queuePtrPtr)->nextTask != NULL; queuePtrPtr = &(((SCHEDULED_TASK*)(*queuePtrPtr))->nextTask)) {
			if( (*queuePtrPtr) == task ) {
				// Task is already scheduled.
				addTask = false;
				break;
			}
			else {
				// Adjust remaining
				switch (task->basePeriod) {
					case CLOCK_PERIODS:
						(*queuePtrPtr)->remaining -= expiredTime;
						break;
					default:
						break;
				}
			}
		}
		if(addTask == true) {
			// Adjust interrupt time if required.
			switch (task->basePeriod) {
				case CLOCK_PERIODS:
					nextmsInterrupt = nextInterruptTime;
					lastInterruptTime = timeNow;
					break;
				default:
					break;
			}
			// Add new task to end of the queue
			(*queuePtrPtr)->nextTask = task;
			task->nextTask = NULL;
		}
	}
	task->task.ctrl |= TASK_TIMER_IS_ACTIVE;
}

/*
 * Remove a task from a timer queue.
 */
void sched_removeTimedTask(SCHEDULED_TASK* task) {
	volatile SCHEDULED_TASK **queuePtrPtr;
	queuePtrPtr = &(timerQueues[task->basePeriod]); // Get the correct task queue
	if((*queuePtrPtr) == task) {
		// First Element, just change head.queuePtr
		*queuePtrPtr = (*queuePtrPtr)->nextTask;
	}
	else {
		for(; (*queuePtrPtr) != NULL; queuePtrPtr = &(((SCHEDULED_TASK*)(*queuePtrPtr))->nextTask)) {
			if((*queuePtrPtr)->nextTask == task) {
				// Remove taskPtr from list
				(*queuePtrPtr)->nextTask = task->nextTask;
				break;
			}
		}
	}
	task->task.ctrl &= ~TASK_TIMER_IS_ACTIVE;
}

/**
 * Add a synchronised task to the task queue to run after a mac frame.
 */
void sched_addSynchronisedTask(volatile TASK* task) {
	volatile TASK *tailPtr;
	if(synchronisedQueueLock) {
		// Queue is locked.
		return;
	}
	if(synchronisedTasks == NULL) {
		synchronisedTasks = task;
		synchronisedTasks->nextTask = NULL;
	}
	else {
		tailPtr = synchronisedTasks;
		while(tailPtr->nextTask != NULL) {
			tailPtr = tailPtr->nextTask;
		}
		tailPtr->nextTask = task;
		task->nextTask = NULL;
	}
	task->ctrl |= TASK_IS_QUEUED;
}

/**
 * Remove a synchronised task from the task queue
 */
void sched_removeSynchronisedTask(volatile TASK* task) {
	volatile TASK *precedingListPtr;
	if(synchronisedQueueLock) {
		// Queue is locked.
		return;
	}
	if(synchronisedTasks == task) {
		synchronisedTasks->nextTask = synchronisedTasks->nextTask;
	}
	else {
		precedingListPtr = synchronisedTasks;
		for(; precedingListPtr != NULL; precedingListPtr = precedingListPtr->nextTask) {
			if(precedingListPtr->nextTask == task) {
				// Remove taskPtr from list
				precedingListPtr->nextTask = task->nextTask;
				break;
			}
		}
	}
	task->ctrl &= ~TASK_IS_QUEUED;
}


/**
 * Add a low priority task to the task queue
 */
void sched_addLowPriorityTask(volatile TASK* task) {
	volatile TASK* tailPtr;
	if(lowPriorityTasks == NULL) {
		lowPriorityTasks = task;
		lowPriorityTasks->nextTask = NULL;
	}
	else {
		tailPtr = lowPriorityTasks;
		while(tailPtr->nextTask != NULL) {
			//If we're already in the queue, don't reschedule
			if(tailPtr == task)
			{
				return;
			}
			tailPtr = tailPtr->nextTask;
		}
		tailPtr->nextTask = task;
		task->nextTask = NULL;
	}
	task->ctrl |= TASK_IS_QUEUED;
}

/**
 * Add a priority task to the task queue.
 * High priority tasks execute before lower ones, timing from queueing to execution may therefore be less.
 * It is really only meant for the MAC protocol.
 * If you can't justify why you need priority over the MAC protocol then don't use this.
 *
 */
void sched_addHighPriorityTask(TASK volatile *task) {
	volatile TASK* tailPtr;
	if(highPriorityTasks == NULL) {
		highPriorityTasks = task;
		highPriorityTasks->nextTask = NULL;
	}
	else {
		tailPtr = highPriorityTasks;
		while(tailPtr->nextTask != NULL) {
			//If we're already scheduled, then why reschedule ourselves?
			if(tailPtr == task)
			{
				return;
			}
			tailPtr = tailPtr->nextTask;
		}
		tailPtr->nextTask = task;
		task->nextTask = NULL;
	}
	task->ctrl |= TASK_IS_QUEUED;
}


/**
 * Scheduler main loop.
 */
void sched_mainLoop(void) {
	volatile TASK *nextTaskPtr;
	volatile TASK *lastTaskPtr;
	volatile TASK *activeTask;
	for(;;) {
		//eint();
		// Check high priority schedule queue.
		if( highPriorityTasks != NULL ) {
			nextTaskPtr = highPriorityTasks;
			while(nextTaskPtr != NULL) {
				nextTaskPtr->execute();
				nextTaskPtr->ctrl &= ~TASK_IS_QUEUED;
				lastTaskPtr = nextTaskPtr;
				nextTaskPtr = lastTaskPtr->nextTask;
				// Remove task from queue.
				lastTaskPtr->nextTask = NULL;
			}
			highPriorityTasks = NULL;
		}
		// Check radio packet pending.
		if( testScheduleFlag(PACKET_PENDING_FLAG) ) {
			sched_unsetScheduleFlag(PACKET_PENDING_FLAG);
		//	packets_stepReceiveQueue();
			continue; // Restart loop as it may have been reset during handling this one.
		}
		// Check synchronised tasks flag.
		if ( testScheduleFlag(SYNCHRONISED_TASKS_FLAG) ) {
			sched_unsetScheduleFlag(SYNCHRONISED_TASKS_FLAG);
			synchronisedQueueLock = true;
			// Run all synchronised tasks
			nextTaskPtr = synchronisedTasks;
			while(nextTaskPtr != NULL) {
				nextTaskPtr->execute();
				nextTaskPtr = nextTaskPtr->nextTask;
			}
			synchronisedQueueLock = false;
			continue;
		}
		// Lower priority task queue. Executed when nothing else is pending.
		// Run one move the pointer through the queue and re-check ISR flags.
		if( lowPriorityTasks != NULL ) {
			activeTask = lowPriorityTasks;
			lowPriorityTasks = lowPriorityTasks->nextTask;
			activeTask->ctrl &= ~TASK_IS_QUEUED;
			activeTask->execute();
			continue;
		}
	}
}

/**
 * Interrupt handler for Timer B compare registers.
 */
void sched_taskTimerISR(unsigned char timer) {
	volatile SCHEDULED_TASK *listPtr, *precedingListPtr;
	volatile TASK *taskPtr;
	unsigned int elapsedTime, lowestRemaining, timeNow;
	TIMER_PERIOD timerBase;

	timerBase = 0xFF;
	timeNow = currentTime;
	
	switch (timer) {
		case 0: // millisecond-incremented tasks
			timerBase = CLOCK_PERIODS;
			elapsedTime = timeNow - lastInterruptTime;
			lastInterruptTime = timeNow; // Record current time to calculate expired time.
			if( timeNow < lastInterruptTime ) {
				elapsedTime += 0xFFFF; // Wrapped around the timer so wadge it forward a bit.
			}
			lowestRemaining = 0xFFFF; // To find soonest timer to expire
			break;
		case 1:	//second-incremented tasks
			timerBase = SECONDS;
			elapsedTime = 1;
			break;
	}
	// Process timers.
	if(timerBase != 0xFF) {
		listPtr = (timerQueues[timerBase]);
		// Decrement elapsed time from queued timers
		precedingListPtr = NULL;
		while(listPtr !=  NULL) {
			listPtr->remaining -= elapsedTime;
			// If this timer has expired then post its task.
			if(listPtr->remaining <= 0) {
				taskPtr = &(listPtr->task);
				// Check task is not NULL.
				if(taskPtr != NULL) {
					if(taskPtr->ctrl & HIGHEST_PRIORITY_FLAG) {
						sched_addHighPriorityTask(taskPtr);
					}
					else {
						sched_addLowPriorityTask(taskPtr);
					}
				}
				else {
					listPtr->period = 0; // Timers with null tasks are removed.
				}
				// If timer has a period then it stays in the list and the remaining is reset to the period. If not, it is a single shot timer or is badly formed so remove it from the list.
				if(listPtr->period > 0) {
					listPtr->remaining = listPtr->period;
					// Move preceding list pointer to this timer to keep track through list.
					precedingListPtr = listPtr;
				}
				else {
					// Remove current entry from the list.
					listPtr->task.ctrl &= ~TASK_TIMER_IS_ACTIVE;
					if(precedingListPtr == NULL) {
						// Current entry is head of the list. Remove by changing head pointer.
						timerQueues[timerBase] = listPtr->nextTask;
					}
					else {
						// Remove this list entry by linking preceding entry to the next one
						precedingListPtr->nextTask = listPtr->nextTask;
					}
					// List entry is removed so preceding pointer does not change.
				}
			}
			// Find soonest timer that will complete. Only check tasks that are not being removed.
			if(listPtr->task.ctrl & TASK_TIMER_IS_ACTIVE) {
				if(listPtr->remaining < lowestRemaining) {
					lowestRemaining = listPtr->remaining;
				}
				// Set list pointer to stop removing tasks that are pending.
				precedingListPtr = listPtr;
			}
			listPtr = listPtr->nextTask; // Move pointer through list until it hits the end and timerPtr == NULL.
		}
		switch( timerBase ) {
			case CLOCK_PERIODS:
				if(timerQueues[CLOCK_PERIODS] == NULL) {
					msDelayTimerActive = false; // Queue is empty, disable interrupt.
				}
				else {
					nextmsInterrupt = timeNow + lowestRemaining; // Set next ISR time
					// Now check for unfortunate circumstances...
					// TODO
				}
				break;
			case SECONDS:
				if(timerQueues[SECONDS] == NULL) {
					secDelayTimerActive = false; // Queue is empty, disable interrupt.
				}
				else {
					nextSecInterrupt += SECONDS_INCREMENT;
				}
				break;
		}
	}
}



